#ifndef WHEEL_FACTOR_H_
#define WHEEL_FACTOR_H_

#include <ceres/ceres.h>

#include "preintegration/wheel_preintegration.h"
#include "common/state.h"

namespace optimizer {

using namespace preintegration;

class WheelFactor : public ceres::SizedCostFunction<3, 1, 2, 1, 2>{
public:  
   WheelFactor(std::shared_ptr<WheelPreIntegration> wheel_preintegration);

   Eigen::Matrix<double, 3, 3> GetCov() {
      return wheel_preintegration_->cov_;
   }

   virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
      double theta_i = parameters[0][0];
      Eigen::Vector2d pi(parameters[1][0], parameters[1][1]);
      double theta_j = parameters[2][0];
      Eigen::Vector2d pj(parameters[3][0], parameters[3][1]);

      Eigen::Matrix2d Ri = SO2(theta_i).matrix();
      Eigen::Vector2d ri = pi;
      double ai = theta_i;
      double aj = theta_j;
      Eigen::Vector2d rj = pj;

      Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);
      residual.setZero();

      residual[0] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();
      residual.tail<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;

      // LOG(INFO) << " wheel residual ==== : " << residual.transpose() << std::endl;
      // LOG(INFO) << " wheel_preintegration_->cov_ \n " << wheel_preintegration_->cov_ << std::endl;

      Eigen::Matrix<double, 3, 3> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 3, 3>>(wheel_preintegration_->cov_.inverse()).matrixL().transpose();
      residual = sqrt_info * residual;
      
      // LOG(INFO) << " wheel residual ==== : " << residual.transpose() << std::endl;

      // LOG(INFO) << "wheel sqrt_info \n " << sqrt_info << std::endl;

      // compute jacobian matrix
      Eigen::Vector2d rij = rj-ri;
      Eigen::Vector2d rij_x(-rij[1], rij[0]);
      // 公式（25）, （十）--- 2.2
      // ti, vxi, vyi, pxi, pyi, bgzi, baxi, bayi, tj, vxj, vyj, pxj, pyj; right
      // 计算雅克比矩阵（修复维度和索引）
      if (jacobians) {
         // 第1个参数块：theta_i（维度1）→ 雅克比 3x1
         if (jacobians[0]) {
               Eigen::Map<Eigen::Matrix<double, 3, 1, Eigen::ColMajor>> jac_theta_i(jacobians[0]);
               jac_theta_i.setZero();
               jac_theta_i(0, 0) = -1;  // 旋转残差对 theta_i 的导数
               jac_theta_i.block<2, 1>(1, 0) = -Ri.transpose() * rij_x; // 位置残差对 theta_i 的导数
               jac_theta_i = sqrt_info * jac_theta_i; // 雅克比加权
         }

         // 第2个参数块：pi（维度2）→ 雅克比 3x2
         if (jacobians[1]) {
               Eigen::Map<Eigen::Matrix<double, 3, 2, Eigen::RowMajor>> jac_pi(jacobians[1]);
               jac_pi.setZero();
               jac_pi.block<2, 2>(1, 0) = -Ri.transpose(); // 位置残差对 pi 的导数
               jac_pi = sqrt_info * jac_pi; // 雅克比加权
         }

         // 第3个参数块：theta_j（维度1）→ 雅克比 3x1
         if (jacobians[2]) {
               Eigen::Map<Eigen::Matrix<double, 3, 1, Eigen::ColMajor>> jac_theta_j(jacobians[2]);
               jac_theta_j.setZero();
               jac_theta_j(0, 0) = 1;  // 旋转残差对 theta_j 的导数
               jac_theta_j = sqrt_info * jac_theta_j; // 雅克比加权
         }

         // 第4个参数块：pj（维度2）→ 雅克比 3x2
         if (jacobians[3]) {
               Eigen::Map<Eigen::Matrix<double, 3, 2, Eigen::RowMajor>> jac_pj(jacobians[3]);
               jac_pj.setZero();
               jac_pj.block<2, 2>(1, 0) = Ri.transpose(); // 位置残差对 pj 的导数
               jac_pj = sqrt_info * jac_pj; // 雅克比加权
         }
      }
   }

private:
   std::shared_ptr<WheelPreIntegration> wheel_preintegration_;
};

}
#endif 
